from astar import AStar
import numpy as np
from scipy.ndimage.filters import gaussian_filter
import matplotlib.pyplot as plt
import math

dem_map = np.zeros((100,100))
# locate mountains
dem_map[80,80] = 50
dem_map[70,30] = 100
dem_map[50,80] = 70
dem_map[20,60] = 100

dem_map = gaussian_filter(dem_map, 10)
dem_map = dem_map*300

class DEMMAPSolver(AStar):
    
    def __init__(self, dem_map):
        self.dem_map = dem_map
        self.width = dem_map.shape[1]
        self.height = dem_map.shape[0]
        
    def heuristic_cost_estimate(self, n1, n2):
        #manhanten
        (x1, y1) = n1
        (x2, y2) = n2
        return abs(x1-x2) + abs(y1-y2)
    
    def distance_between(self, n1, n2):
        (x1, y1) = n1
        (x2, y2) = n2
        if self.dem_map[y2, x2] - self.dem_map[y1, x1] > 0:
            return self.dem_map[y2, x2] - self.dem_map[y1, x1]
        else:
            return 0
    def neighbors(self, node):
        x, y = node
        return[(nx, ny) for nx, ny in[(x, y - 1), (x, y + 1), (x - 1, y), (x + 1, y), (x+1,y+1), (x-1,y-1)]
               if 0 <= nx < self.width and 0 <= ny < self.height]

start = (50, 90)  # we choose to start at the upper left corner
goal = (30, 20)  # we want to reach the lower right corner
foundPath = list(DEMMAPSolver(dem_map).astar(start, goal))

for (x,y) in foundPath:
    dem_map[y,x] = 30
    
plt.matshow(dem_map)
plt.show()


